#pragma once

#include <cmath>
#include <vector>

#include "types.h"
#include "Random.h"

class Boid {
public:
    Boid(double x, double y) {
        acceleration = pi::Point2d(0, 0);

        double angle = pi::Random::RandomInt(0, (int)M_PI*2.0*1000) * 1.0 / 1000.0;
        velocity = pi::Point2d(cos(angle), sin(angle));

        position = pi::Point2d(x, y);
        r = 2.0;
        maxspeed = 2.0;
        maxforce = 0.03;
    }

    virtual ~Boid() {

    }

    void run(std::vector<Boid> &boids) {
        flock(boids);

        update();
        borders();
        render();
    }

    void applyForce(pi::Point2d force) {
        // We could add mass here if we want A = F / M
        acceleration = acceleration + force;
    }

    // We accumulate a new acceleration each time based on three rules
    void flock(std::vector<Boid> &boids) {
        pi::Point2d sep = separate(boids);      // Separation
        pi::Point2d ali = align(boids);         // Alignment
        pi::Point2d coh = cohesion(boids);      // Cohesion

        // Arbitrarily weight these forces
        sep = 1.5 * sep;
        ali = 1.0 * ali;
        coh = 1.0 * coh;

        // Add the force vectors to acceleration
        applyForce(sep);
        applyForce(ali);
        applyForce(coh);
    }

    // Method to update position
    void update() {
        // Update velocity
        velocity = velocity + acceleration;

        // Limit speed
        velocity.limit(maxspeed);
        position = position + velocity;

        // Reset accelertion to 0 each cycle
        acceleration = 0 * acceleration;
    }

    // A method that calculates and applies a steering force towards a target
    // STEER = DESIRED MINUS VELOCITY
    pi::Point2d seek(pi::Point2d target) {
        pi::Point2d desired = target - position;  // A vector pointing from the position to the target

        // Scale to maximum speed
        desired.normalize();
        desired = maxspeed * desired;

        // Above two lines of code below could be condensed with new PVector setMag() method
        // Not using this method until Processing.js catches up
        // desired.setMag(maxspeed);

        // Steering = Desired minus Velocity
        pi::Point2d steer = desired - velocity;
        steer.limit(maxforce);  // Limit to maximum steering force
        return steer;
    }

    void render() {
        /*
        // Draw a triangle rotated in the direction of velocity
        float theta = velocity.heading2D() + radians(90);
        // heading2D() above is now heading() but leaving old syntax until Processing.js catches up

        fill(200, 100);
        stroke(255);
        pushMatrix();
        translate(position.x, position.y);
        rotate(theta);
        beginShape(TRIANGLES);
        vertex(0, -r*2);
        vertex(-r, r*2);
        vertex(r, r*2);
        endShape();
        popMatrix();
        */
    }

    // Wraparound
    void borders() {
        if (position.x < -r) position.x = width+r;
        if (position.y < -r) position.y = height+r;
        if (position.x > width+r)  position.x = -r;
        if (position.y > height+r) position.y = -r;
    }

    // Separation
    // Method checks for nearby boids and steers away
    pi::Point2d separate(std::vector<Boid> &boids) {
        double desiredseparation = 25.0f;
        pi::Point2d steer = pi::Point2d(0, 0);
        int count = 0;
        // For every boid in the system, check if it's too close
        for (Boid other : boids) {
            float d = position.dist(other.position);
            // If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)
            if ((d > 0) && (d < desiredseparation)) {
                // Calculate vector pointing away from neighbor
                pi::Point2d diff = position - other.position;
                diff.normalize();
                diff = diff / d;    // Weight by distance
                steer = steer + diff;

                count++;            // Keep track of how many
            }
        }
        // Average -- divide by how many
        if (count > 0) {
            steer = steer / (double) count;
        }

        // As long as the vector is greater than 0
        if (steer.norm() > 0) {
            // First two lines of code below could be condensed with new PVector setMag() method
            // Not using this method until Processing.js catches up
            // steer.setMag(maxspeed);

            // Implement Reynolds: Steering = Desired - Velocity
            steer.normalize();
            steer = steer * maxspeed;
            steer = steer - velocity;
            steer.limit(maxforce);
        }
        return steer;
    }

    // Alignment
    // For every nearby boid in the system, calculate the average velocity
    pi::Point2d align (std::vector<Boid> &boids) {
        float neighbordist = 50;
        pi::Point2d sum(0, 0);
        int count = 0;
        for (Boid other : boids) {
            double d = position.dist(other.position);
            if ((d > 0) && (d < neighbordist)) {
                sum = sum + other.velocity;
                count++;
            }
        }

        if (count > 0) {
            sum = sum / (double) count;

            // First two lines of code below could be condensed with new PVector setMag() method
            // Not using this method until Processing.js catches up
            // sum.setMag(maxspeed);

            // Implement Reynolds: Steering = Desired - Velocity
            sum.normalize();
            sum = sum * maxspeed;
            pi::Point2d steer = sum - velocity;
            steer.limit(maxforce);
            return steer;
        }
        else {
            return pi::Point2d(0, 0);
        }
    }

    // Cohesion
    // For the average position (i.e. center) of all nearby boids, calculate steering vector towards that position
    pi::Point2d cohesion (std::vector<Boid> &boids) {
        float neighbordist = 50;
        pi::Point2d sum(0, 0);   // Start with empty vector to accumulate all positions
        int count = 0;
        for (Boid other : boids) {
            float d = position.dist(other.position);
            if ((d > 0) && (d < neighbordist)) {
                sum = sum + other.position; // Add position
                count++;
            }
        }
        if (count > 0) {
            sum = sum / (double) count;
            return seek(sum);  // Steer towards the position
        }
        else {
            return pi::Point2d(0, 0);
        }
    }

public:
    pi::Point2d position;
    pi::Point2d velocity;
    pi::Point2d acceleration;

    double      r;
    double      maxforce;               ///< Maximum steering force
    double      maxspeed;               ///< Maximum speed

    int         width, height;
};
